Unified framework for precise vision-aided navigation

ABSTRACT

A system and method for efficiently locating in 3D an object of interest in a target scene using video information captured by a plurality of cameras. The system and method provide for multi-camera visual odometry wherein pose estimates are generated for each camera by all of the cameras in the multi-camera configuration. Furthermore, the system and method can locate and identify salient landmarks in the target scene using any of the cameras in the multi-camera configuration and compare the identified landmark against a database of previously identified landmarks. In addition, the system and method provide for the integration of video-based pose estimations with position measurement data captured by one or more secondary measurement sensors, such as, for example, Inertial Measurement Units (IMUs) and Global Positioning System (GPS) units.

CROSS-REFERENCE TO RELATED APPLICATION

This application is a continuation of co-pending U.S. patent applicationSer. No. 11/949,433 filed Dec. 3, 2007, which claims the benefit of U.S.Provisional Patent Application 60/868,227 filed Dec. 1, 2006, both ofwhich are incorporated herein by reference in their entireties.

STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT

The U.S. Government has a paid-up license in this invention and theright in limited circumstances to require the patent owner to licenseothers on reasonable terms as provided for by the terms of Contract No.M67854-05-C-8006 awarded by the Office of Naval Research.

FIELD OF THE INVENTION

The present invention relates generally to visual odometry, particularlyto a system and method for enhanced route visualization in missionplanning applications.

BACKGROUND OF THE INVENTION

Precise and efficient navigation systems are very important for manyapplications involving location identification, route planning,autonomous robot navigation, unknown environment map building, etc.However, most of the available navigation systems do not functionefficiently and fail frequently under certain circumstances.

For example, GPS (Global Positioning System) is a widely used navigationsystem. However, GPS is limited in that it cannot work reliably once thesatellite signals are blocked or unavailable in, for example,“GPS-denied” environments such as indoors, forests, urban areas, etc.Even when operating properly, GPS can only provide the location of theuser, which is not sufficient to assist the user during navigation. Forexample, when a group of military personnel are performing a militaryoperation in an unknown environment, it is desirable to determine notonly the location of each member of the unit, but also identify whateach user in the group is seeing, where they are looking (i.e., whetherthey are viewing a region or target of interest, and the movement and/ordirection of the user's visual perspective), and any other activity andhappenings in the scene. In addition, it is highly desirable to have asystem wherein the individual members of the team are able to sharevisual and navigational information with one another.

In addition, conventional systems including multiple cameras or othervisual sensing devices provide for limited ability to performcomprehensive visual odometry. Specifically, in such systems, visualodometry can be applied to each camera individually to estimate the poseof that camera. As such, the relative poses of the cameras (i.e., acamera's pose relative to the other cameras in the system) are fixed andknown, which constrains the single-camera results. In this regard,conventional navigation systems provide for the generation of camerapose hypotheses that are based exclusively on data which is specific tothe individual camera, and does not take into account the data processedby the other camera's in the multi-camera system.

Therefore, there is a need for a method and a system for efficiently andeffectively determine navigational information using a multi-camerasystem.

SUMMARY OF THE INVENTION

The above-described problems are addressed and a technical solution isachieved in the art by a system and method for providing real-timecapture of visual sensor data. According to an embodiment of the presentinvention, a vision-based navigation system is provided which canidentify the 3D coordinates (i.e., determine a location in 3D) of anobject of interest (e.g., a person, vehicle, building, etc.) with alocal reference coordinate system in a GPS-denied or GPS-challengedenvironment. In this regard, the vision-based navigation system andmethod of the present invention can use sparse GPS readings and landmarkinformation from the environment to geo-locate an object of interest(e.g., a person, vehicle, etc.) in 3D.

According to the an embodiment of the present invention, the systems andmethods are configured to independently estimate the position and 3Dorientation accurately by using only video images (e.g., image streams)captured from one, two or more inexpensive video cameras. The system,just as would a human, is able to track features in the environment overtime to localize the position and gaze of the sensor system in 3D. Thesystem is generally illustrated in FIG. 1, which shows a number of usersequipped with video cameras configured to track features (e.g., trees,vehicles, buildings) in a target scene.

Moreover, the vision-based navigation system according to an embodimentof the present invention can be integrated with one or more secondarymeasurement sensors, such as, for example, an inertial measurement unit(IMU), GPS unit, and/or suitable rigidly mounted video sensors toimprove localization accuracy and robustly determine both the locationand the 3D gaze or orientation of the user under a wide range ofenvironments and situations.

According to an embodiment of the present invention, the systems andmethods are configured to integrate video-based pose estimates based onvideo captured by one or more video cameras; position measurement datacaptured by one or more secondary measurement sensors, such as, forexample, IMUs and GPS units. The position measurement data may include,for example, 3D acceleration and angular rate measurements captured byan IMU and/or a positioning reading captured by GPS. The integratedinformation may be used to generate a refined pose estimation and isadapted to detect outliers and failures in the video component orsecondary measurement sensor components of the system.

According to an embodiment of the present invention, a framework isprovided which improves visual odometry performance by recognizingpreviously visited locations and using information related to theprevious visits to correct drift accumulated during navigation.Specifically, at each traveled location along a route, a set ofdistinctive visual landmarks is automatically extracted from the targetscene and inserted dynamically into a landmark database. As the usertravels to a new location, a new set of visual landmarks is extractedfrom the scene and the landmark database searched for their possiblematches. Using the previously identified and recognized visual landmarksduring location re-visits, an improved pose correction technique isprovided which can significantly reduce or eliminate the accumulatednavigation drift.

According to an embodiment of the invention, a real-time multi-camerabased visual odometry can locate an individual or other target objectwith precision, for example, within 1 meter over 1 km in an unknownenvironment.

Additionally, the systems and methods of the present invention areconfigured to record detailed information and imagery associated with anenvironment in real-time. The stored information and imagery can beshared and analyzed to assist the user in reporting specific informationregarding what the user is viewing, herein referred to as the “userviewpoint.”

FIG. 2 illustrates an exemplary screenshot from a displaycommunicatively connected to the vision-based navigation system of thepresent invention. As shown, as a user equipped with the vision-basednavigation system travels through the environment, his or her positionand viewpoint can be located precisely in the map, preferably in areal-time manner, as shown in the image in the upper right of FIG. 2. Inaddition, the user viewpoint (i.e., what is seen from the user'sperspective and location) in the context of a larger environment isdenoted by the shaded triangular shaped portion of the image on the lefthand side of the screenshot shown in FIG. 2.

The video images captured by the user are fed to an image processingsystem configured to analyze the images and detects objects of interest,such as, for example, vehicles and people. The location(s) of the one ormore objects of interest are recorded in a database and can beidentified in a map of the environment. The video data and estimatedpose information are stored in a database, preferably in real-time, andcan be played and processed off-line to assist in planning andimplementing future navigation tasks, mission reporting and missionrehearsal.

According to an embodiment of the present invention, a global landmarkrecognition system and method are provided which can locate salientlandmarks in the environment from any one video sensor and match theidentified landmark against previously identified landmarks stored in adatabase (herein referred to as the “landmark database”). The previouslyidentified landmarks, which were extracted from video at a differentpoint in time (same or different video sensor), can be used to correlatethe position information with respect to a global coordinate systemmaintained by the landmark database.

Advantageously, according to an embodiment, the systems and methods ofthe present invention are configured for efficient extract and storelandmarks and related information in the landmark database over a longsequence. Subsequently, the stored landmarks can be matched withlandmarks detected in real-time by a user operating a wearablevision-based navigation system illustrated in FIG. 3.

The present invention further relates to a system and method forlandmark recognition. According to an embodiment of the invention, thesystem and method are configured to receive from a video camera a videosnapshot comprising a pair of stereo images (i.e., a right image and aleft image) related to a target scene. Next, a set of natural featurepoints (in 2D) are extracted from the right and left images. Once the 2Dfeature points are extracted, a stereo matching algorithm is activatedto identify correspondences among the extracted set of feature pointsbetween the right and left images. The 3D coordinates of each featurepoint are computed according to any suitable method known in the art,thereby producing a representation of the landmark. The 2D and 3Dcoordinates of the feature(s) are herein referred to as the “spatialconfiguration data.”

After the one or more landmarks have been detected from the stereo pair,a histogram of oriented gradients (HOG) descriptor is computed based onat least a portion of the stereo image pair. According to an embodimentof the present invention, instead of computing the HOG descriptor with afixed scale to select the image patch size, as in conventional methods,here, the HOG descriptor is computed with a depth-dependent scale. Thedepth-dependent scale from the stereo images is used to define a scalefor the landmarks to make the features highly robust.

The HOG descriptor (i.e., a fixed scale or depth-dependent scale HOGdescriptor) is associated with the landmark to produce a “landmarksnapshot.” According to an embodiment of the present invention, thelandmark snapshot may comprise the associated HOG descriptor along withthe spatial configuration data (i.e., the 2D and 3D coordinates relatedto the one or more features).

According to an embodiment of the present invention, given two imagestaken at different locations, landmarks can be extracted from the imagesand matched using the HOG descriptors. In one embodiment, the landmarkmatching is implemented as a very efficient pipeline that utilizesspatio-temporal distribution of the points to prune candidate posehypothesis for matching. In addition, the landmark matching can be usedto correlate routes and locations with a mission and between multiplemissions. Similar routes taken by multiple soldiers can be correlatedtogether though the matching process.

BRIEF DESCRIPTION OF THE DRAWINGS

The present invention will be more readily understood from the detaileddescription of exemplary embodiments presented below considered inconjunction with the attached drawings, of which:

FIG. 1 illustrates an exemplary environment for use of the vision-basednavigation systems and methods according to embodiments of the presentinvention;

FIG. 2 illustrates an exemplary screenshot generated by the vision-basednavigation systems and methods of the present invention;

FIG. 3 shows a prototype vision-based navigation system that can be wornby a user, according to an embodiment of the present invention;

FIG. 4 shows a vision-based navigation system and functional flowdiagram, according to an embodiment of the present invention;

FIG. 5 illustrates a functional flow diagram related to a multi-camerapreemptive Random Sample Consensus (RANSAC) system and process,according to an embodiment of the present invention;

FIG. 6 illustrates a functional flow diagram related to a multi-camerapose refinement system and process, according to an embodiment of thepresent invention;

FIG. 7 illustrates a function flow diagram related to a globallandmark-based pose estimation system and process, according to anembodiment of the present invention;

FIG. 8 illustrates a landmark extraction system and method, according toan embodiment of the present invention; and

FIG. 9 illustrates an exemplary histogram computation of 3D landmarkdistributions generated according to an embodiment of the presentinvention.

It is to be understood that the attached drawings are for purposes ofillustrating the concepts of the invention and may not be to scale.

DETAILED DESCRIPTION OF THE INVENTION

The present invention relates to vision-based navigation systems andmethods for determining location and navigational information related toa user and/or other object of interest. An overall system and functionalflow diagram according to an embodiment of the present invention isshown in FIG. 4. The systems and methods of the present inventionprovide for the real-time capture of visual data using a multi-cameraframework and multi-camera visual odometry (described below in detailwith reference to FIGS. 3, 5, and 6); integration of visual odometrywith secondary measurement sensors (e.g., an inertial measurement unit(IMU) and/or a GPS unit) (described in detail with reference to FIG. 3);global landmark recognition including landmark extraction, landmarkmatching, and landmark database management and searching (described indetail with reference to FIGS. 3, 7, 8, and 9).

As shown in FIG. 4, the vision-based navigation system 1 (hereinreferred to as the “Navigation System”) according to an embodiment ofthe present invention comprises a plurality of video sensors (e.g.,cameras) 10 herein referred to a “Camera Cluster” or “plurality ofcameras”; a Visual Odometry Module 20; an Integration Module 30; aLandmark Extractor 40; a Landmark Matcher 50; and a Landmark Database60. The components of the Navigation System 1 may be comprised of,accessible by, or implemented by one or more computers and relatedcomputer-executable programs. The term “computer” is intended to includeany data processing device, such as a desktop computer, a laptopcomputer, a mainframe computer, a personal digital assistant, a server,a handheld device, or any other device able to process data. Thecommunicative connections, inter-operation, and functionality of eachcomponent of the Navigation System 1 is described in detail below. Theterms “communicative connection” and “communicatively connected” areintended to include any type of connection, whether wired or wireless,in which data may be communicated. The term “communicatively connected”is intended to include a connection between devices and/or programswithin a single computer or between devices and/or programs on separatecomputers.

With reference to FIG. 4, the Camera Cluster 10 comprises a plurality ofvideo sensors (e.g., cameras) configured to capture video related to anenvironment, herein referred to as the “target scene.” The target scenemay comprise one or more features, including, but not limited to,people, vehicles, buildings, etc. Each of the plurality of cameras isconfigured to capture video from that camera's perspective, hereinreferred to as a “local camera capture.”

The captured video from each of the cameras is provided by the CameraCluster 10 to the Visual Odometry Module 20, as shown in FIG. 4. TheVisual Odometry Module 20 is a computer-based program configured toperform multi-camera visual odometry wherein pose information istransferred across different cameras of the Camera Cluster 10 such thatvideo-based pose hypotheses generated by each camera can be evaluated bythe entire system. In operation, a set of pose hypotheses (also referredto as pose candidates) are generated by each camera in the CameraCluster 10 based on an evaluation of the local camera capture against alocal dataset (i.e., the dataset of the local camera). In addition, eachset of pose hypotheses is transferred to and evaluated by the othercameras in the Camera Cluster 10, resulting in the evaluation of theeach camera's pose hypotheses on a global dataset and enabling therobust selection of an ‘optimized’ pose.

In order for the multi-camera visual odometry method of the presentinvention to be performed, a pose transfer mechanism is provided whichtransfers each camera's pose to all of the other cameras in the CameraCluster 10. The pose transfer or transformation process is described ingreater detail below in the context of an example wherein the CameraCluster 10 includes 2 cameras (referred to as “Camera j” and “Camerak”). One having ordinary skill in the art will appreciate that theexample including two cameras is intended for illustrative purposes, andthat systems having greater than two cameras are within the scope of thepresent invention.

According to an embodiment of the present invention, the pose (P) ofCamera j in the world at time t may be represented by a rotation matrixR_(wj)(t) and a translation vector T_(w) ^(j)(t) that map the worldpoint X_(w) to the camera point x_(j):

X _(j) =R _(wj)(t)X _(w) +T _(w) ^(j)(t).  (Equation 1)

This transformation can also be expressed as

${\begin{pmatrix}X_{j} \\1\end{pmatrix} = {{P_{wj}(t)}\begin{pmatrix}X_{w} \\1\end{pmatrix}}},{{{where}\mspace{14mu} {P_{wj}(t)}} = \begin{pmatrix}{R_{wj}(t)} & {T_{w}^{j}(t)} \\0 & 1\end{pmatrix}}$

Similarly, the pose (P) of Camera k relative to Camera j can beexpressed as P_(jk), such that:

$\begin{matrix}{\begin{pmatrix}X_{k} \\1\end{pmatrix} = {{P_{jk}\begin{pmatrix}X_{j} \\1\end{pmatrix}}.}} & \left( {{Equation}\mspace{14mu} 2} \right)\end{matrix}$

According to an embodiment of the present invention, it may be assumedthat both the intrinsic calibration parameters and the relative posesP_(jk) between every camera pair on the rig are available through aninitial calibration. The relationship can be expressed asP_(jk)=P_(wk)(t)P_(jw)(t) which is assumed fixed in time, by expressingthe same world point in terms of Camera j and Camera k coordinates. Assuch, P_(jw)(t)=P_(wj) ⁻¹(t). Then, assuming P_(wj)(t) and P_(jk) areknown, the relationship can be expresses as follows:

P _(wk)(t)=P _(jk) P _(wj)(t).  (Equation 3)

According to a single mono or stereo visual odometry algorithm, at timestep t₁ the camera pose for Camera j the coordinate system of thatcamera at time t₀ can be obtained, and denoted as P_(j)(t₀,t₁), thematrix which transforms coordinates for Camera j at time t₀ tocoordinates for Camera j at time t₁. As such, the relationship may beexpressed as follows:

P _(j)(t ₀ ,t ₁)=P _(wj)(t ₁)P _(jw)(t ₀)  (Equation 4)

Given P_(j)(t₀,t₁) and the relative pose P_(jk), the transformationmatrix P_(k)(t₀,t₁) may be computed according to the followingexpressions:

$\begin{matrix}\begin{matrix}{{P_{k}\left( {t_{0},t_{1}} \right)} = {{P_{wk}\left( t_{1} \right)}{P_{kw}\left( t_{0} \right)}}} \\{= {P_{jk}{P_{wj}\left( t_{1} \right)}{P_{jw}\left( t_{0} \right)}P_{jk}^{- 1}}} \\{= {P_{jk}{P_{j}\left( {t_{0},t_{1}} \right)}{P_{kj}.}}}\end{matrix} & \left( {{Equation}\mspace{14mu} 5} \right)\end{matrix}$

In terms of the rotation matrix R and translation vector T, this resultcan be written as:

R _(k)(t ₀ ,t ₁)=R _(jk) R _(j)(t ₀ ,t ₁)R _(kj),  (Equation 6)

T _(k)(t ₀ ,t ₁)=R _(jk) R _(j)(t ₀ ,t ₁)T _(k) ^(j) +T _(j) ^(k) +R_(jk) T _(j)(t ₀ ,t ₁).  (Equation 7)

where T_(j) ^(k)=−R_(jk)T_(k) ^(j).

According to an embodiment of the present invention, the aboverelationship for cameras that are rigidly connected on a fixed platform(rig) with a known configuration allows for the determination of theposes of all the cameras constrained by any given single camera pose.

With reference to FIG. 4, the Visual Odometry Module 20 receivescaptured video from each of the cameras in the Camera Cluster 10 (inthis example, Camera j and Camera k) and performs feature detection andtracking, independently in each camera, as described in an articletitled “Visual Odometry,” by D. Nister, et al., in Proc. IEEE Conferenceon Computer Vision and Pattern Recognition, Volume 1, Issue 27, Pages1-652-1-659 (2004), the entire contents of which are hereby incorporatedby reference herein. The feature detection and tracking involveextraction of multiple key points in each video frame for all thecameras in the system and finding matches between the frames. Accordingto an embodiment of the present invention, ‘Harris corners’ may beidentified according to the Harris Corner Extraction process, atechnique well-known in the art. However, one having ordinary skill inthe art will appreciate that any feature which describes an image pointin a distinctive manner can be used. Each feature correspondenceestablishes a feature track, which lasts as long as the feature remainsmatched in the new frames that are acquired in the video sequence. Asold tracks vanish, new ones are established with features that have notbeen previously observed. The feature tracks for each camera areprovided into a program and/or module configured to perform a poseestimation process.

Based on the feature tracks, pose hypotheses are generated according toa multi-camera preemptive RANSAC method, described in detail herein. Inthe following section, the multi-camera preemptive RANSAC method isdescribed for a multi-camera monocular system. One having ordinary skillin the art will appreciate that a multi-stereo system behaves much likethe same way, except that, for relative pose calculations a 5-pointalgorithm over three views is performed in the monocular scheme, whereasa 3-point algorithm is performed in the stereo scheme using the 3D pointcloud determined by stereo triangulation.

According to an embodiment of the invention, the preemptive RANSACprocess is applied to a monocular visual odometry system. Using randomlyselected 5 point correspondences over three views, N number of posehypotheses (by default N=500) are generated using the 5-point algorithm,wherein each pose hypothesis consists of the pose of the second andthird view with respect to the first view. Then, starting with all thepose hypotheses (also referred to as video-based pose hypotheses), theyare evaluated on sets of M data points based on trifocal Sampson error(by default M=100), each time dropping out half of the least scoringhypotheses. Thus, the initial set of hypotheses includes 500 posehypotheses, all of which are evaluated on a subset of 100-pointcorrespondences (i.e., the local dataset). Next, the pose hypotheses aresorted according to their scores based on the local dataset, and thebottom half (i.e., the half having the lowest scores) is discarded.

In the next step, a set local dataset including 100 data points isselected on which the remaining 250 pose hypotheses are evaluated andthe least scoring half are pruned and so forth until a singlebest-scoring hypothesis remains. One having ordinary skill in the artwill appreciate that the numbers (i.e., the size of the local datasubsets, the initial number of pose hypotheses, the number discarded periteration) provided above are intended merely for illustrative purposes,and that other numbers may be used and are within the scope of thepresent invention. The stereo case is very similar to the above, excepta 3-point resection algorithm over randomly chosen 3 pointcorrespondences between 3D world points (obtained by stereotriangulation) and 2D image points is used to generate the posehypotheses, and a robust cost function of reprojection errors is used induring the scoring phase.

FIG. 5 illustrates a multi-camera preemptive RANSAC process, accordingto an embodiment of the present invention. For a multi-camera system, asdescribed above, each camera independently generates 500 pose hypothesesthat are based on randomly selected 5 point correspondences using itsown data. However, the pose hypotheses generated by each camera areevaluated not only on the data of the camera that generated them (hereinthe “local dataset”) but their transformations (described in detailabove) are also evaluated based on each of the rest of the cameras inthe system, after they have been transformed properly throughout thesystem. In this regard, the pose estimations generated by each cameramay be scored on a “global” dataset, i.e., the data maintained by theother cameras in the system. Accordingly, an “intra-camera score” isdetermined based on an evaluation of the pose estimation against its ownlocal dataset. Furthermore, each pose is transformed according to thepose transfer relationship expressed in Equation 5 above, and scored onevery other camera's local dataset in the multi-camera system togenerate one or more “intra-camera scores”).

The preemptive scoring in each camera is accomplished by obtaining a“global” score for each hypothesis by combining its corresponding scoresdetermined in all the cameras (i.e., the intra-camera score and the oneor more inter-camera scores) on the initial set of 100 data points fromeach camera. Next, a portion of the set of pose hypotheses having thelowest global scores (e.g., half of the initial set) are discarded andthe remaining half is evaluated on another set of 100 points in everycamera. The global scores are updated, and the process continuesiteratively until an optimized pose hypothesis is identified. Uponcompletion of the multi-camera preemptive RANSAC process, the posehypothesis having the highest global score is identified in each camera(herein referred to as the optimized pose hypothesis).

According to an embodiment of the present invention, the multi-camerapreemptive RANSAC process may include the selection of 5 points acrossdifferent cameras to compute the initial set of pose hypotheses. This isin contrast to the embodiment described above wherein the 5-pointcorrespondences are selected in each camera individually. Implementingan algorithm that selects these point matches from different cameras andsolves for the entire system pose satisfying those constraints furtherenhances the performance of the system by taking direct advantage of thewide field of view provided by the multi-camera system.

Following identification of the optimized pose hypothesis for eachcamera, the Visual Odometry Module 20 performs a multi-camera poserefinement process on the said hypotheses, as shown in FIG. 6. In boththe monocular and stereo visual odometry algorithms, the best pose forthat camera at the end of the preemptive RANSAC routine is passed to apose refinement (polishing) step where iterative minimization of arobust cost function (derived from Cauchy distribution) of thereprojection errors is performed using the known Levenberg-Marquardtmethod, as described in “Visual Odometry,” by D. Nister, et al., inProc. IEEE Conference on Computer Vision and Pattern Recognition, Volume1, Issue 27, Pages 1-652-1-659 (2004). In the monocular case, for thepose refinement over three views in each camera, a bundle adjustment iscarried out where the poses of the middle and last view are refinedalong with the 3D points, wherein the poses are in the first viewcoordinate frame that is fixed at identity. The initial estimate for theposes is obtained as the result of the 5-point RANSAC method asdescribed above. In the above example, the middle view pose has beenobtained by resection using the 3D-2D correspondences, where the 3Dpoints have been obtained by triangulation between first and last views.Also the reprojection error in the middle view has been used to selectthe best pose solution out of the multiple pose solutions from the5-point method.

In the bundle adjustment process, the error for each image point in eachview is a robust function of the reprojection error. In particular, thereprojection error of the i'th point in the j'th view is,e_(ij)=x_(ij)−h(P_(j)X_(i)), where x_(ij) is the inhomogeneous imagecoordinates of the feature point, P_(j) is the pose matrix, and X_(i) isthe 3D point in homogeneous coordinates. Function h is the conversionfrom homogeneous into inhomogeneous coordinates. The robust function ofthe error is log(1+|e_(ij)|²/s²), where s is the scale parameter of theCauchy density determined by the user.

In the stereo case, for the pose refinement of the left stereo view ineach camera, a robust function of the reprojection error in the new leftand right views is used as the objective function, and a 3D point cloudis obtained by stereo triangulation of the previous left and right viewswith known poses. The initial estimate for the new left and right viewsis obtained by the 3-point RANSAC method described above.

As shown in FIG. 6, the pose refinement system and method is generalizedin the multi-camera system and the polishing is distributed acrosscameras in the following way: a) for each camera, the optimized posehypothesis (i.e., the hypothesis having the highest global score) isrefined not only on the camera from which it originated but also on allthe cameras after it is transferred. Then, the global scores of therefined hypotheses in each camera are computed and the best cumulativescoring polished hypothesis is determined. This pose hypothesis isstored in the camera from which it originated (or it is transferred ifthe best pose comes from a different camera than the original). Therefinement process is repeated for all the cameras in the system,resulting in each camera having a refined pose hypothesis. As a result,the process allows for the scenario where a given camera pose is moreeffectively refined by a camera other than the one in which itoriginated. Finally, the pose hypothesis which has the highest globalscore, is selected and applied to the system.

According to an embodiment of the present invention, the Visual OdometryModule 20 can be configured to compare the global scores at the end ofthe refinement process or after every iteration of the refinementprocess, by computing the optimized pose hypothesis and transferring itto all the cameras for the next iteration. It is noted, that in thedescription below, the pose hypotheses described above which aregenerated by the Visual Odometry Module 20 are referred to as“video-based pose hypotheses,” for clarity in the discussion below of“landmark-based pose hypotheses.”

As shown in FIG. 4, the Navigation System 1 comprises an IntegrationModule 30 which is communicatively connected to the Visual OdometryModule 20 and one or more Secondary Measurement Sensors 35. In order toincrease the robustness of the Navigation System 1, the SecondaryMeasurement Sensors 35 (e.g., a Microelectromechanical (MEMS) Systemssuch as an IMU and/or a GPS unit) provides position measurement data(e.g., 3D acceleration and angular rate measurements captured by an IMUand/or a positioning reading captured by GPS) to the Integration Module30 for integration with the video-based pose hypotheses generated by theVisual Odometry Module 20. According to an embodiment of the presentinvention, the Integration Module 30 can be comprised of an ExtendedKalman Filter (EKF) framework. In this framework, a constant velocityand a constant angular velocity model are selected for the filterdynamics. The state vector consists of 13 elements: X, (3-vector)representing position in navigation coordinates, q, unit quaternion(4-vector) for attitude representation in navigation coordinates, ν,(3-vector) for translational velocity in body coordinates, and ω,(3-vector) for rotational velocity in body coordinates. Quaternionrepresentation for attitude has several practical properties. Eachcomponent of the rotation matrix in quaternion is algebraic, eliminatingthe need for transcendental functions. It is also free of thesingularities that are present with other representations and theprediction equations are treated linearly. Based on this, the processmodel is represented as follows:

X _(k) =X _(k−1) +R ^(T)(q _(k−1))X _(rel)

q _(k) =q _(k−1)

q(ρ_(rel))

ω_(k)=ω_(k−1) +n _(ω,k−1)

ν_(k)=ν_(k−1) +n _(ν,k−1)

where

X _(rel)=ν_(k−1) Δt _(k) +n _(ν,k−1) Δt _(k)

ρ_(rel)=ω_(k−1) Δt _(k) +n _(ω,k−1) Δt

and

is used to denote the quaternion product operation; ρ is the rotationvector (representing the rotation axis) in the body frame; R(q) is therotation matrix determined by the attitude quaternion q in thenavigation frame; and q(ρ) is the quaternion obtained from the rotationvector ρ. Undetermined accelerations in both translational and angularvelocity components are modeled by zero mean white Gaussian noiseprocesses n_(ν,k) and n_(ω,k). The filter runs at the frame rate,meaning that the discrete time index denoted by k corresponds to theframe times when pose outputs are also available from visual odometrysystem.

According to an embodiment of the present invention, the angular ratesensor (gyro) and accelerometer readings from the IMU are used directlyas measurements in the Integration Module 30. The multi-camera visualodometry frame to frame local pose measurements expressed in thecoordinate frame of the front left camera, P_(k)=P(t_(k),t_(k+1)), arealso converted to velocities by extracting the rotation axis vectorcorresponding to the rotation matrix R_(k), together with the cameratranslation given by R^(T)T_(k), (where P_(k)=[R_(k); T_(k)]) and thendividing by the timestep, Δt_(k)=t_(k+1)−t_(k). Hence, the informationfrom the Visual Odometry Module 20 and the IMU are used according to thefollowing measurement model:

ν_(k) ^(νo)=ν_(k) +n _(ν,k) ^(νo)

ω_(k) ^(νo)=ω_(k) +n _(ω,k) ^(νo)

ω_(k) ^(imu)=ω_(k) +n _(ω,k) ^(imu)

a _(k) ^(imu) =R(q _(k))g+n _(a,k) ^(imu)

X _(k) ^(gps) =X _(k) +n _(k) ^(gps)

where ν_(k) ^(νo) and ω_(k) ^(νo) are translational and angular velocitymeasurements provided by the Visual Odometry Module 20 (vo); w_(k)^(imu) and a_(k) ^(imu) are the gyro and accelerometers outputs providedby the IMU; X_(k) ^(gps) is the position measurement provided by the GPSunit; g is the gravity vector. Uncertainty in the video-based posehypotheses, represented by the noise components n_(ν,k) ^(νo) andn_(ω,k) ^(νo), is estimated based on the reprojection error covarianceof image features through backward propagation.

The gyro noise errors are modeled with fixed standard deviation valuesthat are much higher than those corresponding to the visual odometrynoise when the pose estimates are good (which is most often the case)and are comparable in value or sometimes much less when vision basedpose estimation is difficult for brief durations. This allows theIntegration Module 30 (i.e., the EKF) to effectively combine the twomeasurements at each measurement update, relying more on the sensor withthe better noise characteristics. During the filter operation, badmeasurements from all sensors (i.e., the cameras, the IMU, and the GPSunit) are rejected using validation mechanisms based on Chi-square testson the Kalman innovations. In addition, those measurements from theVisual Odometry Module 20 causing large accelerations are alsodiscarded. In order to obtain the initial position and attitude of thecamera in navigation coordinates, first roll and pitch outputs from theIMU are used directly, and heading is obtained by the aid of the firsttwo GPS readings that are sufficiently spread apart.

In certain circumstances, there may be a high degree of agreementbetween the angular velocities computed by visual odometry alone andthose available from the gyros. However, if the gyro angular velocitymeasurements are used alone, e.g., by removing the second equation fromthe above measurement model, then a very large drift in the overalltrajectory is observed. As such, although when viewed on a graph, thedifference between the angular velocities at each time instant is small(at those times when visual odometry is working efficiently), and theerrors due to the gyro bias add up quickly over time. On the other hand,visual odometry trajectory is very accurate except for brief regionswhere it might “break” causing gross errors in the global sense. It isduring these “breaks” that it is optimal to integrate positionmeasurement data from the one or more Secondary Measurement Sensors 35.

To avoid corrupting the visual odometry measurements at times when theprocess is operating efficiently, the difference in velocities in allthree rotation-axes is computed at each time instant and compares to athreshold. If this difference in all axes is smaller than the threshold,then the third equation of the measurement model set forth above isremoved. This also serves as a double check on the quality of the visualodometry output, meaning that if it is close to the gyro output wechoose the visual odometry alone. Therefore, in those cases, theuncertainty in the visual odometry estimate computed by thebackpropagation method described above is much smaller than the gyro,thereby making the Kalman gain on the gyro much smaller. By using theabove approach, corruption from the IMU may be eliminated at thoseinstances. Further, by selecting process model noise covariances thatare high, the filter essentially passes the visual odometry outputsuntouched, replacing the constant velocity based prediction.

Following integration of the IMU and GPS (Secondary Measurement Sensors35) with the video-based pose estimation produced by the Visual OdometryModule 20, the Integration Module 30 generates a refined poseestimation. The refined pose estimation is passed from the IntegrationModule 30 to the Landmark Matcher 50. Although the refined poseestimation provided by the Integration Module 30 is highly accurate, itmay, in certain circumstances, include drift from the true trajectorydue to accumulation of errors over time. Therefore, a globallandmark-based recognition and route correction for multi-camera visualodometry framework comprising the Landmark Extractor 40 and the LandmarkMatcher 50) is provided that integrates visual landmark recognitiontechniques to correct any accumulated drift errors, as illustrated inFIG. 7.

As shown in FIG. 7, given a time stamp t, the first step of the proposedalgorithm is to detect and track a set of natural landmarks from theimages of the forward and backward stereo pairs individually.Subsequently, with the use of the extracted landmarks, the proposedmulti-stereo camera based pose estimation technique is activated toestimate the video-based pose estimation (P_r). Simultaneously, theextracted landmarks from both stereo pairs at the current time stamp (t)are used to search the landmark database for their most similarlandmarks via the efficient database matching technique described indetail below. Once a set of similar landmarks is returned, a new“landmark-based pose estimation” (P_l) is estimated by comparing thecurrent image position of the one or more landmarks with the 3D locationstored in the database. The drift error accumulated in the video-basedpose estimation P_r is corrected by the landmark-based pose estimationP_l to obtain a “final pose estimation” (P_c). Note if the matchedlandmarks are not found in the database then the new landmarks are addedto the Landmark Database 60 dynamically.

FIG. 8 illustrates an exemplary flowchart of the steps performed by theLandmark Extractor 40 based on a pair of stereo images at each time t,according to an embodiment of the present invention. According to anembodiment of the present invention, the Landmark Extractor 40 isconfigured to perform Harris Corner extraction—wherein given a pair ofstereo images at time t, a set of natural feature points are detectedfrom the images. The Harris corner detector is utilized to automaticallyextract a set of feature points from the left and right imagesrespectively, as described in detail in U.S. patent application Ser. No.11/833,498, titled “A Stereo-Based Visual Odometry Method and System,”by Zhu et al., filed Aug. 3, 2007 (herein the '498 Application), theentire disclosure of which is hereby incorporated by reference herein.Advantageously, the Harris corners are relatively stable under small tomoderate image distortions, as described in the '498 Application.

The Landmark Extractor 40 is further configured to perform a stereomatching process, wherein once the feature points are extracted, astereo matching algorithm is activated to find correspondences among theextracted feature points between left and right images. The 3Dcoordinates of each feature point are subsequently computed bytriangulation using the feature correspondences. The estimated 3Dfeature points serve as the landmarks for the stereo pair.

After the landmarks are detected from the stereo pair, the HOGdescriptor is computed from the left image of the stereo pair torepresent each landmark. According to an embodiment of the presentinvention, instead of computing the HOG descriptor with a fixed scale toselect the image patch size, the scale S of the HOG descriptor for eachlandmark is determined automatically via the following equation:

$S = {S_{ref} \times \frac{Z_{ref}}{Z}}$

where Z is the depth or the coordinate in the Z direction of thelandmark, and S_(ref) is scale used for the landmark whose depth isequal to Z_(ref). According to an exemplary embodiment, S_(ref) may beset to/and Z_(ref) is set to 10 meters heuristically. Therefore, thecloser the landmark, the larger the scale will be given. Once thedepth-dependent HOG descriptor is computed, each landmark is associatedwith the 2D coordinates, 3D correspondences and a HOG descriptor (i.e.,either a fixed scale or depth-dependent scale HOG descriptor).

As described above, each image is represented by a set of extractedlandmarks with the HOG descriptors. Given two images taken at differentlocations, the Landmark Matcher 50 is configured to match the extractedlandmarks between them using the HOG descriptors. According to anembodiment of the present invention, the landmark matching processcomprises, for each landmark in the first image, search all thelandmarks in the second image for its correspondence. The search isbased on the cosine similarity score of the HOG descriptors between twolandmarks. A landmark in the second image that produces the highestsimilarity score is considered as a potential match for the landmark inthe first image. At the same time, for each landmark in the secondimage, a potential match in the first image can also be assignedautomatically with the one that produces the highest similarity score.Finally, only the pairs that mutually have each other as the potentialmatches are accepted a valid match.

According to an embodiment of the present invention, the landmarkmatching process may further comprise the use of epipolar geometryconstraints to eliminate the false matches. Specifically, from theobtained matches between two images, a fundamental matrix F is firstestimated via the robust RANSAC technique described in detail in the'498 Application. Subsequently, based on the estimated fundamentalmatrix F, those matches that produce residuals that are larger than apredefined threshold value are treated as false matches and discarded.

As illustrated in FIG. 3, the Landmark Matcher 50 (and optionally, theLandmark Extractor 40) is communicatively connected to the LandmarkDatabase 60. In operation, during navigation, the local sceneinformation at one particular point along the route is captured by theraw images or “video snapshots” taken by the plurality of video cameras.For simplicity, a unique set of representative landmarks is extractedfrom the images to represent the scene at each location along the routedynamically. Therefore, instead of inserting the raw images of the sceneat each location into a database directly, all the extracted landmarksfrom the scene are added to the Landmark Database 60.

As a result, the Landmark Database 60 comprises a list of videosnapshots of the landmarks extracted from the scene at each locationalong the route during navigation. Since each landmark is represented bya distinctive HOG descriptor, the combination of HOG descriptors withthe spatial configuration data (i.e., 2D and 3D coordinate data) of thelandmarks creates a distinctive “landmark snapshot” which serves as thefingerprint of the location and landmark. Each landmark snapshot iscomposed of the 2D coordinates, 3D coordinates and HOG descriptors ofthe landmarks, and, optionally, the estimated 3D camera location.

According to an embodiment of the present invention, the LandmarkDatabase 60 serves as the “memory” of all the places that the usertraveled. Accordingly, when the user arrives at a new location, theextracted landmarks from the scene are utilized to search the LandmarkDatabase 60 to decide whether the new location has been visited before.Since the size of the Landmark Database 60 increases rapidly as thetravel continues, an efficient database searching strategy is needed, asis described below.

According to an embodiment of the present invention, an efficienthierarchy database searching strategy is provided which comprises thefollowing steps, each of which are described in detail below: 1)landmark match candidate selection comprising the use of geo-spatialconstraints; 2) candidate pruning comprising the use of a 3D landmarkspatial distribution process; and 3) HOG-based landmark matchingcomprising the use of chronological constraints.

According to an embodiment of the present invention, candidate landmarkmatches are selected using geo-spatial constraints. In operation, whenthe user travels to a new location, based on its estimated 3D location,a geo-spatial search region is obtained. Subsequently, with the use ofthe geo-spatial search region, an initial set of candidate landmarksnapshots is obtained quickly from the Landmark Database 60 and storedin a temporary memory, herein referred to as a landmark candidate cache.

Since the number of selected candidates in the landmark candidate cacheis determined and limited by the size of the geo-spatial search region,which is typically small, the large-scaled Landmark Database 60 (whichmay include thousands or more entries) to a small candidate cache (e.g.,several hundred candidates) during the landmark database searchingprocess. Optionally, according to an embodiment of the presentinvention, the geo-spatial search region can be set to be sufficientlylarge and it can automatically vary with the drift rate or uncertainty.

Next, the initial set of candidate landmark snapshots is pruned orreduced to identify a refined set of candidate landmark snapshots,according to an embodiment of the present invention. Typically, the 3Dspatial distribution of the landmarks extracted from the scene varieswith the locations along the route. Therefore, a histogram vector iscomputed to characterize the 3D spatial distribution of the landmarks ateach snapshot in the landmark candidate cache. For example, according toan embodiment of the present invention, the histogram vector can consistof 16 bins, wherein each bin counts the number of landmarks distributedin the space determined by the depth information, as illustrated in FIG.9.

Next, the histogram vector of 3D landmark distributions of the currentlandmark snapshot is used to compare with each candidate in landmarkcandidate cache. Finally, a smaller candidate cache (usually less thanone hundred or none) can be obtained by discarding those candidates withlow level of similarities.

Once a smaller candidate cache is obtained, a HOG descriptor-basedlandmark matching process is activated to search the candidate cache toidentify the optimized candidate landmark snapshot. In operation, sincethe same environment/target scene may be visited several times, theremay be several snapshots of the landmarks associated with theenvironment/target scene. According to embodiments of the presentinvention, the landmark matching process may terminate when it identifythe first match or may continue until it identifies the most ‘similar’landmarks. Optionally, a chronological constraint may be integrated intothe process to further expedite the search. Specifically, the candidatelandmark snapshots can be first ranked according to the time they arecaptured. Once the candidate landmark snapshot that satisfies thesimilarity measurement is returned (i.e., the optimized candidatelandmark snapshot), the search termites, and the optimized candidatelandmark identified and returned.

As described above, according to an embodiment of the present invention,the Landmark Matcher 50 can be configured to perform a globallandmark-based pose correction to generated a final pose estimation. Inoperation, once a landmark snapshot is returned from the LandmarkDatabase 60, it may mean that the user is visiting an environment/targetscene that has been visited previously. As a result, with the use ofrecognized landmarks serving as the reference points, a new pose Pr ofthe visual odometry system at the current position can be recomputeddirectly.

It is to be understood that the exemplary embodiments are merelyillustrative of the invention and that many variations of theabove-described embodiments may be devised by one skilled in the artwithout departing from the scope of the invention. It is thereforeintended that all such variations be included within the scope of thefollowing claims and their equivalents.

1. A computer implemented landmark recognition method comprising:capturing a plurality of images where the plurality of images comprisesa view of one or more landmarks obtained by each of a plurality ofcameras, where each camera in the plurality of cameras has differentposition measurement data comprising location, depth and pose ascompared to position measurement data of each other camera; computing alandmark snapshot for each landmark of the one or more landmarks in theplurality of images, the landmark snapshot comprising a scale identifierfor the landmark combined with a spatial configuration for the landmark;and comparing between the landmark snapshot for each of the one or morelandmarks in the plurality of images to identify corresponding landmarksin the plurality of images.
 2. The method of claim 1, wherein a set ofpose hypotheses for each camera in the plurality of cameras aregenerated based on randomly selected 3 point correspondences over twoviews of a corresponding camera in the plurality of cameras.
 3. Themethod of claim 1 wherein the scale identifier is a histogram oforiented gradients (HOG) descriptor.
 4. The method of claim 1 whereinthe scale identifier is a fixed scale identifier.
 5. The method of claim1 wherein the scale identifier is a depth-dependent scale identifier. 6.The method of claim 1 further comprising: scoring a set of posehypotheses generated for each camera based on a comparison within alocal dataset of point correspondences for each camera within theplurality of cameras to produce a set of intra-camera scores; andidentifying an optimized video-based pose hypothesis for each camerabased on an evaluation of a set of global scores generated bytransferring the set of pose hypotheses generated for each camera toother cameras in the system and producing a set of inter-camera scores.7. The method of claim 6, further comprising integrating the posehypotheses with the position measurement data.
 8. The method of claim 7wherein the integrating comprising performing a Kalman filteringoperation on the position measurement data and the pose hypotheses. 9.The method of claim 6, further comprising determining positionalmeasurement data using an inertial measurement unit (IMU).
 10. Themethod of claim 6, further comprising determining positional measurementdata using a global positioning system (GPS).
 11. The method of claim 6,further comprising refining the identified landmark matches byestimating a fundamental matrix using a RANSAC method, and based on theestimated fundamental matrix discarding one or more matches that producea residual that is higher than a predefined threshold value.
 12. Themethod of claim 11, wherein the set of pose hypotheses are generatedbased on randomly selected 5 point correspondences over three views ofeach camera within the plurality of cameras using local camera datacorresponding to each camera.
 13. The method of claim 12, wherein theset of pose hypotheses are generated based on randomly selected 5 pointcorrespondences over three views using data from the plurality ofcameras.
 14. The method of claim 1 wherein the comparing between thelandmark snapshot further comprises: determining correspondence oftwo-dimensional features in the landmark snapshot for each of the one ormore landmarks in the plurality of images for reconstructingthree-dimensional coordinates for the two-dimensional features;determining scale for each scale identifier that is proportional to thedistance between each camera within the plurality of cameras and thelandmark snapshot associated with each camera; comparing a cosinesimilarity score of the scale identifier to determine if one or morelandmarks correspond.
 15. The method of claim 1 further comprising:pruning the landmark snapshot for each of the one or more landmarks toidentify a refined set of candidate landmark snapshots; and computing ahistogram vector corresponding to the spatial distribution of thelandmarks in each landmark snapshot.
 16. The method of claim 15 furthercomprising ranking the candidate landmark according to time of capture.